/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * ROBOMODULE MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes            	Mail
 * 2020-08-22     WangXi       first version    	WangXi_chn@foxmail.com
 * 
 * Note:
 * 
 */
 
#include "Module_RobomoduleGroup.h"

/* User Code Begin*/


/* User Code End */

/* Static Method */

static void 	Module_RobomoduleGroupInit(MODULE_ROBOMODULEGROUP *module);
static void 	Module_RobomoduleGroupSend(MODULE_ROBOMODULEGROUP *module);
static void 	Module_RobomoduleGroupFeed(MODULE_ROBOMODULEGROUP *module);
static rt_err_t Module_RobomoduleGroupHandle(rt_device_t dev, rt_size_t size);

static rt_err_t Module_Robomodule_Config(MODULE_ROBOMODULE *module);
static void 	Module_RobomoduleInit(MODULE_ROBOMODULE *module);

static struct rt_semaphore canRobomodule_sem;


/* Global Method */
rt_err_t Module_RobomoduleGroup_Config(MODULE_ROBOMODULEGROUP *module)
{
	if( module->Method_Init		==NULL &&
		module->Method_Feed		==NULL &&
		module->Method_Send		==NULL &&
		module->Method_Handle	==NULL
		
    ){  
        /* Link the Method */
        module->Method_Init 	= Module_RobomoduleGroupInit;
		module->Method_Feed		= Module_RobomoduleGroupFeed;
		module->Method_Send		= Module_RobomoduleGroupSend;
		module->Method_Handle	= Module_RobomoduleGroupHandle;
			
    }
    else{
        rt_kprintf("Warning: Module Robomodule is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    module->Method_Init(module);
	
	/* Motor Config */
	for(int i=0;i<8;i++)
	{
		if(module->Value_robomodule[i].Property_Enable!= 0)
		{
			module->Value_moduleUsedMask |= 0x01<<i; 
			module->Value_robomodule[i].Value_can_dev = module->Value_can_dev;
			Module_Robomodule_Config(&(module->Value_robomodule[i]));
		}
	}

    return RT_EOK;
}

static void Module_RobomoduleGroupInit(MODULE_ROBOMODULEGROUP *module)
{
	rt_err_t res;
	
	/* link can devices */
	module->Value_can_dev = rt_device_find(module->Property_CanDevName);
	if (!module->Value_can_dev)
        rt_kprintf("find %s failed!\n", module->Property_CanDevName);
	
	/* init semaphone */
	rt_sem_init(&canRobomodule_sem, "canRobomodule_sem", 0, RT_IPC_FLAG_FIFO);
		
	res = rt_device_open(module->Value_can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
	RT_ASSERT(res == RT_EOK);	
	
	/* set up can baudrate */
	rt_device_control(module->Value_can_dev, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
	
	/* link call back function */
	rt_device_set_rx_indicate(module->Value_can_dev, module->Method_Handle);
}

rt_err_t Module_Robomodule_Config(MODULE_ROBOMODULE *module)
{
	if( module->Method_Init		==NULL
		
    ){  
        /* Link the Method */
        module->Method_Init 	= Module_RobomoduleInit;
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    module->Method_Init(module);
	
    return RT_EOK;
}

/* Static Method */
static void Module_RobomoduleInit(MODULE_ROBOMODULE *module)
{	
	/* Reset the motor */
	module->Value_can_msg.id  = (module->Property_GroupID<<8)|((module->Property_NumID+1)<<4);
	module->Value_can_msg.id  += 0x000;
	module->Value_can_msg.ide = RT_CAN_STDID;
	module->Value_can_msg.rtr = RT_CAN_DTR;
	module->Value_can_msg.len = 8;
	for(int i=0;i<8;i++)
		module->Value_can_msg.data[i]=0x55;
	rt_size_t size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msg), sizeof(module->Value_can_msg));
	if (size == 0)
		rt_kprintf("can dev write data failed!\n");
	
	rt_thread_mdelay(500);
	
	/* Set the motor mode*/
	module->Value_can_msg.id  = (module->Property_GroupID<<8)|((module->Property_NumID+1)<<4);
	module->Value_can_msg.id  += 0x001;
	module->Value_can_msg.ide = RT_CAN_STDID;
	module->Value_can_msg.rtr = RT_CAN_DTR;
	module->Value_can_msg.len = 8;
	module->Value_can_msg.data[0]=module->Property_Mode;
	for(int i=1;i<8;i++)
		module->Value_can_msg.data[i]=0x55;	
	size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msg), sizeof(module->Value_can_msg));
	if (size == 0)
		rt_kprintf("can dev write data failed!\n");
	
	rt_thread_mdelay(500);
	
	module->Value_motor_AimAngle = 0;
	module->Value_motor_AimCurrent = 0;
	module->Value_motor_AimRPM = 0;
}

static void Module_RobomoduleGroupSend(MODULE_ROBOMODULEGROUP *module)
{
	rt_size_t  size;
	
	/* Send motor data */	
	for(int i=0;i<8;i++)
	{
		if((module->Value_moduleUsedMask & (0x01<<i))!=0x00)
		{
			switch(module->Value_robomodule[i].Property_Mode)
			{							
				case ROBOMODULE_MODE_OPENLOOP:					
					break;
				case ROBOMODULE_MODE_CURRENT:					
					break;
				case ROBOMODULE_MODE_SPEED:					
					break;
				case ROBOMODULE_MODE_LOCATION:
					module->Value_can_msg.id  
						=(module->Value_robomodule[i].Property_GroupID<<8)
						|((module->Value_robomodule[i].Property_NumID+1)<<4);
					module->Value_can_msg.id  += 0x005;
					module->Value_can_msg.ide = RT_CAN_STDID;
					module->Value_can_msg.rtr = RT_CAN_DTR;
					module->Value_can_msg.len = 8;
					module->Value_can_msg.data[0]= (unsigned char)(5000>>8)&0xFF;
					module->Value_can_msg.data[1]= (unsigned char)(5000)&0xFF;
					module->Value_can_msg.data[2]=0x55;
					module->Value_can_msg.data[3]=0x55;
					module->Value_can_msg.data[4]= 
						(unsigned char)((module->Value_robomodule[i].Value_motor_AimAngle>>24)&0xff);
					module->Value_can_msg.data[5]= 
						(unsigned char)((module->Value_robomodule[i].Value_motor_AimAngle>>16)&0xff);
					module->Value_can_msg.data[6]= 
						(unsigned char)((module->Value_robomodule[i].Value_motor_AimAngle>>8)&0xff);
					module->Value_can_msg.data[7]= 
						(unsigned char)(module->Value_robomodule[i].Value_motor_AimAngle&0xff);
				
					size = rt_device_write(module->Value_can_dev, 0, 
							&(module->Value_can_msg), sizeof(module->Value_can_msg));
					if (size == 0)
						rt_kprintf("robomodule can dev write data failed!\n");
	
					break;
				case ROBOMODULE_MODE_SPEEDLOCATION:					
					break;
				case ROBOMODULE_MODE_CURRENTSPEED:					
					break;
				case ROBOMODULE_MODE_CURRENLOCATION:					
					break;
				case ROBOMODULE_MODE_CURRENSPEEDLOCATION:					
					break;
			}			
		}		
	}
}

static rt_err_t Module_RobomoduleGroupHandle(rt_device_t dev, rt_size_t size)
{
	rt_sem_release(&canRobomodule_sem);
		
	return RT_EOK;
}

static void Module_RobomoduleGroupFeed(MODULE_ROBOMODULEGROUP *module)
{	
	rt_sem_take(&canRobomodule_sem, RT_WAITING_FOREVER);
	
	/* Get true value of the motor */
	rt_device_read(module->Value_can_dev, 0, &(module->Value_can_mrg), sizeof(module->Value_can_mrg));
	
}


/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/
